// reveive mavros topic /mavros/rc/in (in type mavros_msgs/RCIn)
// watch the chanel 8, when it come from 1094 to 1934,
// publish a topic waypoint_generator/waypoints (in type  nav_msgs::Path)

#include <ros/ros.h>
#include <mavros_msgs/RCIn.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>

class ReceiveRC
{
public:
  ReceiveRC()
  {
    rc_sub = nh.subscribe<mavros_msgs::RCIn>(
        "/mavros/rc/in", 10, &ReceiveRC::rcCallback, this);
    path_pub = nh.advertise<nav_msgs::Path>(
        "/waypoint_generator/waypoints", 10);
  }

  void rcCallback(const mavros_msgs::RCIn::ConstPtr &msg)
  {
    if (rc_flag) return;

    if (msg->channels[7] == 1934)
    {
      rc_flag = true;
      path.header.stamp = ros::Time::now();
      pose.pose.position.x = 0;
      pose.pose.position.y = 0;
      pose.pose.position.z = 0;
      pose.pose.orientation.x = 0;
      pose.pose.orientation.y = 0;
      pose.pose.orientation.z = 0;
      pose.pose.orientation.w = 1;
      path.poses.push_back(pose);
      path_pub.publish(path);
    }
  }

private:
  ros::NodeHandle nh;
  ros::Subscriber rc_sub;
  ros::Publisher path_pub;
  nav_msgs::Path path;
  geometry_msgs::PoseStamped pose;
  bool rc_flag{false};
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "reveiveRC");
  ReceiveRC reveiveRC;
  ros::spin();
  return 0;
}